#include "WPILib.h"

#ifndef __ROBOT2489_h__
#define __ROBOT2489_h__

#define KICKERREADYINGTIME 0.4 //time it takes for the kicker to charge OR relax OR anti-charge, should be very small
#define LATCHOPENTIME .4 //time it takes for latch to open AND kick to actually occur.
#define KICKERRETRACTTIME 0.8 //time it takes for kicker piston to retract
#define LATCHCLOSETIME 0.2 //time it takes for latch to close so kicking will be ready again.
#define FORWARDDRIVETIME 1 //autonomous forward drive increments

class Robot2489 : public SimpleRobot
{
	Jaguar *leftMotorController;
	Jaguar *rightMotorController;
	RobotDrive *myRobot; // robot drive system
	Joystick *rightStick; // joystick 1 (arcade stick or right tank stick)
	Joystick *leftStick; // joystick 2 (tank left stick)
	Joystick *manipStick;
	DriverStation *ds; // driver station object
	Compressor *KarlKompressor; // this is a compressor
	Solenoid *solKickOut; // this is the left solenoid
	Solenoid *solKickIn; // this is the right solenoid
	Solenoid *solLatch; //this is the latch solenoid
	Solenoid *solTower; //this solenoid controls BOTH of the tower's pistons
	//Task *kickTask;
	Timer *kickTimar;
	DriverStationLCD *screen;
	DriverStationEnhancedIO *dseio;
	
	
	bool kickTriggered;

	/*enum // Driver Station jumpers to control program operation
	{	RELOADED, // Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
		KICKERCHARGED, // Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous)
		LATCHOPENED,
		KICKERRETRACTED,
		LATCHCLOSED,
	} kickerState;*/
	
	enum // Driver Station jumpers to control program operation
	{	ARCADE_MODE = 1, // Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
		ENABLE_AUTONOMOUS = 2, // Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous)
	} jumpers;
	
	/*enum
	{
		READY;
		KICKERCHARGED;
		LATCHOPENED;
		KICKERRETRACTED;
		LATCHCLOSED;
	} kickerState;*/
/*private:
	static Robot2489* m_instance;
	static int s_kickFunction(void);*/
public:
	Robot2489(void);
	~Robot2489(void);
	void Autonomous(void);
	void OperatorControl(void);
	//static Robot2489& getInstance(void);
	void kickFunction(void);
	void ChargeKicker(void);
	void RetractKicker(void);
	void OpenLatch(void);
	void CloseLatch(void);
	void RelaxKicker(void);
	double MotorControl(double);
	void DriveForward(float, float);
	void DriveBackward(float,float);
	void TurnRight();
	void RaiseTower();
	void RetractTower();
};

#endif
